// math3d.js -- Copyright 2007 Google

// Some Javascript math utilities.
//
// Exports V3 (3-vector utilities), M33 (3x3 matrix utilities)

// NOTE: This will be refactored in a more Object
// Oriented style, so don't get attached to this syntax!

// 3D vector functions.
V3 = {
	EARTH_RADIUS: 6378100,

	dup: function(a) {
		return [a[0], a[1], a[2]];
	},

	toString: function(a) {
		return "[" + a[0] + ", " + a[1] + ", " + a[2] + "]";
	},

	nearlyEqual: function(a, b, tolerance) {
		if (!tolerance) {
			tolerance = 1e-6;
		}
		return Math.abs(a[0] - b[0]) <= tolerance
			&& Math.abs(a[1] - b[1]) <= tolerance
			&& Math.abs(a[2] - b[2]) <= tolerance;
	},
	
	cross: function(a, b) {
		return [
				a[1] * b[2] - a[2] * b[1],
				a[2] * b[0] - a[0] * b[2],
				a[0] * b[1] - a[1] * b[0] ];
	},

	dot: function(a, b) {
		return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
	},

	add: function(a, b) {
		return [
				a[0] + b[0],
				a[1] + b[1],
				a[2] + b[2]];
	},

	sub: function(a, b) {
		return [
				a[0] - b[0],
				a[1] - b[1],
				a[2] - b[2]];
	},

	scale: function(a, scale) {
		return [a[0] * scale, a[1] * scale, a[2] * scale];
	},

	length: function(a) {
		return Math.sqrt(a[0] * a[0] + a[1] * a[1] + a[2] * a[2]);
	},

	normalize: function(a) {
		var len = V3.length(a);
		if (len <= 0) {
			return [NaN, NaN, NaN];
		}
		return V3.scale(a, 1.0 / len);
	},

	bisect: function(a, b) {
		return [(a[0] + b[0]) / 2,
						(a[1] + b[1]) / 2,
						(a[2] + b[2]) / 2];
	},

	// Returns v rotated counterclockwise about axis by radians.
	// axis should be a unit vector; otherwise you'll get weird results.
	rotate: function(v, axis, radians) {
		var vDotAxis = V3.dot(v, axis);
		var vPerpAxis = V3.sub(v, V3.scale(axis, vDotAxis));
		var vPerpPerpAxis = V3.cross(axis, vPerpAxis);
		var result = V3.add(V3.scale(axis, vDotAxis),
												V3.add(V3.scale(vPerpAxis, Math.cos(radians)),
															 V3.scale(vPerpPerpAxis, Math.sin(radians))));
		return result;
	},

	// Takes a set of Euler angles and converts from degrees to radians.
	toRadians: function(v) {
		return [v[0] * Math.PI / 180,
						v[1] * Math.PI / 180,
						v[2] * Math.PI / 180];
	},

	// Takes a set of Euler angles and converts from radians to degrees.
	toDegrees: function(v) {
		return [v[0] * 180 / Math.PI,
						v[1] * 180 / Math.PI,
						v[2] * 180 / Math.PI];
	},

	// Input is [lat, lon, alt].	Lat & lon are in degrees, positive up
	// and east.	Alt in meters, relative to Earth's radius.
	//
	// Output is meters x,y,z.	x points out of (0,0) (just off West
	// Africa), y points out the North Pole, and z points out of (0,-90)
	// (near Ecuador).
	latLonAltToCartesian: function(vert) {
		var sinTheta = Math.sin(vert[1] * Math.PI / 180);
		var cosTheta = Math.cos(vert[1] * Math.PI / 180);
		var sinPhi = Math.sin(vert[0] * Math.PI / 180);
		var cosPhi = Math.cos(vert[0] * Math.PI / 180);

		var r = V3.EARTH_RADIUS + vert[2];
		var result = [
				r * cosTheta * cosPhi,
				r * sinPhi,
				r * -sinTheta * cosPhi ];
		return result;
	},

	// Input is meters [x, y, z].	Output is [lat, lon, alt].	Lat & lon
	// in degrees, alt in meters.
	// 
	// V3.cartesianToLatLonAlt([R, 0, 0]) ~= [0, 0, 0]
	// V3.cartesianToLatLonAlt([R/sqrt(2), R/sqrt(2), 0]) ~= [45, 0, 0]
	// V3.cartesianToLatLonAlt([R/sqrt(2), 0, R/sqrt(2)]) ~= [0, -45, 0]
	cartesianToLatLonAlt: function(a) {
		var r = V3.length(a);
		if (r <= 0) {
			return [NaN, NaN, NaN];
		}
		var alt = r - V3.EARTH_RADIUS;
		// Compute projection onto unit sphere.
		var n = V3.scale(a, 1 / r);
		var lat = Math.asin(n[1]) * 180 / Math.PI;
		if (lat > 90) {
			lat -= 180;
		}
		var lon = 0;
		if (Math.abs(lat) < 90) {
			lon = Math.atan2(n[2], n[0]) * -180 / Math.PI;
		}
		return [lat, lon, alt];
	},

	// Return the signed perpendicular distance from the point c to the line
	// defined by [a, b].
	//
	// We get the sign by determining if point is to the left of the line,
	// from the point of view of looking towards the origin through vert0.
	// I.e. is it to the left, looking at the surface of the Earth from
	// above.
	leftDistance: function(a, b, c) {
		var ab = V3.sub(b, a);
		var ac = V3.sub(c, a);
		var cross = V3.cross(ab, ac);

		var dot = V3.dot(a, cross);
		var lineLength = V3.length(ab);
		if (lineLength < 1e-6) {
			return NaN;
		}
		var perpendicularDistance = V3.length(cross) / lineLength;

		if (dot > 0) {
			return perpendicularDistance;
		} else {
			return -perpendicularDistance;
		}
	},

	// Return the distance between two cartesian 3d points, along the
	// surface of the Earth, assuming they are on the surface of the
	// Earth.	(If the inputs are not on the surface of the earth, they
	// are projected to the surface first.)
	earthDistance: function(a, b) {
		var dot = V3.dot(V3.normalize(a), V3.normalize(b));
		var angle = Math.acos(dot);
		var dist = V3.EARTH_RADIUS * angle;
		return dist;
	}
};

M33 = {
	// Conventions:
	//
	// * V3 is a 3-element array representing a column vector
	//
	// * M33 is an array of 3 column vectors, representing a 3x3 matrix
	//
	//	 [ [00] [10] [20] ]
	//	 [ [01] [11] [21] ]
	//	 [ [02] [12] [22] ]
	
	toString: function(a) {
		return "[" + V3.toString(a[0]) + ", " + 
			V3.toString(a[1]) + ", " + V3.toString(a[2]) + "]";
	},

	nearlyEqual: function(a, b) {
		return V3.nearlyEqual(a[0], b[0])
			&& V3.nearlyEqual(a[1], b[1])
			&& V3.nearlyEqual(a[2], b[2]);
	},

	transpose: function(a) {
		return [
				[a[0][0], a[1][0], a[2][0]],
				[a[0][1], a[1][1], a[2][1]],
				[a[0][2], a[1][2], a[2][2]]];
	},

	multiply: function(a, b) {
		var result = [[0, 0, 0], [0, 0, 0], [0, 0, 0]];
		for (var i = 0; i < 3; i++) {
			for (var j = 0; j < 3; j++) {
				result[i][j] = a[0][j] * b[i][0]
											 + a[1][j] * b[i][1]
											 + a[2][j] * b[i][2];
			}
		}
		return result;
	},

	// Applies matrix a to column vector b.	(I.e. returns a * b)
	transform: function(a, b) {
		return [
				a[0][0] * b[0] + a[1][0] * b[1] + a[2][0] * b[2],
				a[0][1] * b[0] + a[1][1] * b[1] + a[2][1] * b[2],
				a[0][2] * b[0] + a[1][2] * b[1] + a[2][2] * b[2]];
	},

	// Applies the transpose of matrix a to column vector b.
	// (I.e. returns a.transpose() * b)
	transformByTranspose: function(a, b) {
		return [
				a[0][0] * b[0] + a[0][1] * b[1] + a[0][2] * b[2],
				a[1][0] * b[0] + a[1][1] * b[1] + a[1][2] * b[2],
				a[2][0] * b[0] + a[2][1] * b[1] + a[2][2] * b[2]];
	},

	identity: function() {
		return [[1, 0, 0], [0, 1, 0], [0, 0, 1]];
	},

	makeOrthonormalFrame: function(dir, up) {
		var newright = V3.normalize(V3.cross(dir, up));
		var newdir = V3.normalize(V3.cross(up, newright));
		var newup = V3.cross(newright, newdir);
		return [newright, newdir, newup];
	},

	// [heading, tilt, roll] (degrees) to [[right],[dir],[up]] (local
	// coords).	The return value transforms global direction vectors
	// into local direction vectors.	The transpose of the return value
	// transforms local direction vectors into global direction vectors.
	//
	// heading, tilt, roll are in degrees, clockwise about z,x,y axes (!?).
	// heading of 0 means pointing North
	// heading of 90 means pointing West
	//
	// [1, 0, 0] in local coords points right (East, for 0, 0, 0 htr)
	// [0, 1, 0] in local coords points ahead (North, for 0, 0, 0 htr)
	// [0, 0, 1] in local coords points up (away from center of earth)
	headingTiltRollToLocalOrientationMatrix: function(htr) {
		return M33.eulToMat(V3.toRadians(htr));
	},

	// [[right], [dir], [up]] (in local cartesian coords) to [heading, tilt, roll]
	// (in degrees)
	localOrientationMatrixToHeadingTiltRoll: function(mat) {
		var htr = M33.matToEul(mat);
		return V3.toDegrees(htr);
	},

	// Builds a local orientation matrix, to transform from local coords
	// into global coords.	"Local" means that the local x basis vector
	// points East, the y basis vector points North and the z basis
	// vector points Up (towards the sky).
	makeLocalToGlobalFrame: function(latLonAlt) {
		var vertical = V3.normalize(V3.latLonAltToCartesian(latLonAlt));
		var east = V3.normalize(V3.cross([0, 1, 0], vertical));
		var north = V3.normalize(V3.cross(vertical, east));

		return [east, north, vertical];
	},

	// See Graphics Gems IV, Chapter III.5, "Euler Angle Conversion" by
	// Ken Shoemake.
	//
	// http://vered.rose.utoronto.ca/people/spike/GEMS/GEMS.html
	eulerConfig: {
		i: 2, j: 0, k: 1,		 // NOTE: KML convention is Z, X, Y!
		counterClockwise: true,
		sameAxis: false,			// third axis same as first (i == k)
		frameRelative: false	// frame-relative (vs. static)
	},

	// Construct matrix from Euler angles (in radians).
	//
	// Thanks to Ken Shoemake / Graphics Gems
	eulToMat: function(eulerAnglesIn) {
		var ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
		var config = M33.eulerConfig;
		var i = config.i;
		var j = config.j;
		var k = config.k;

		var ea = V3.dup(eulerAnglesIn);
		var m = [[0, 0, 0], [0, 0, 0], [0, 0, 0]];
		if (config.frameRelative) { var t = ea[0]; ea[0] = ea[2]; ea[2] = t; }
		if (!config.counterClockwise) {
			ea[0] = -ea[0]; ea[1] = -ea[1]; ea[2] = -ea[2];
		}
		ti = ea[0];		tj = ea[1];	th = ea[2];
		ci = Math.cos(ti); cj = Math.cos(tj); ch = Math.cos(th);
		si = Math.sin(ti); sj = Math.sin(tj); sh = Math.sin(th);
		cc = ci*ch; cs = ci*sh; sc = si*ch; ss = si*sh;
		if (config.sameAxis) {
			m[i][i] = cj;		 m[i][j] =	sj*si;		m[i][k] =	sj*ci;
			m[j][i] = sj*sh;	m[j][j] = -cj*ss+cc; m[j][k] = -cj*cs-sc;
			m[k][i] = -sj*ch; m[k][j] =	cj*sc+cs; m[k][k] =	cj*cc-ss;
		} else {
			m[i][i] = cj*ch; m[i][j] = sj*sc-cs; m[i][k] = sj*cc+ss;
			m[j][i] = cj*sh; m[j][j] = sj*ss+cc; m[j][k] = sj*cs-sc;
			m[k][i] = -sj;	 m[k][j] = cj*si;		m[k][k] = cj*ci;
		}
		return m;
	},

	// Convert matrix to Euler angles (in radians).
	//
	// Thanks to Ken Shoemake / Graphics Gems
	matToEul: function(m, config) {
		var config = M33.eulerConfig;
		var i = config.i;
		var j = config.j;
		var k = config.k;

		var FLT_EPSILON = 1e-6;
		var ea = [0, 0, 0];
		if (config.sameAxis) {
			var sy = Math.sqrt(m[i][j] * m[i][j] + m[i][k] * m[i][k]);
			if (sy > 16 * FLT_EPSILON) {
				ea[0]= Math.atan2(m[i][j], m[i][k]);
				ea[1]= Math.atan2(sy, m[i][i]);
				ea[2]= Math.atan2(m[j][i], -m[k][i]);
			} else {
				ea[0]= Math.atan2(-m[j][k], m[j][j]);
				ea[1]= Math.atan2(sy, m[i][i]);
				ea[2]= 0;
			}
		} else {
			var cy = Math.sqrt(m[i][i] * m[i][i] + m[j][i] * m[j][i]);
			if (cy > 16 * FLT_EPSILON) {
				ea[0]= Math.atan2(m[k][j], m[k][k]);
				ea[1]= Math.atan2(-m[k][i], cy);
				ea[2]= Math.atan2(m[j][i], m[i][i]);
			} else {
				ea[0]= Math.atan2(-m[j][k], m[j][j]);
				ea[1]= Math.atan2(-m[k][i], cy);
				ea[2]= 0;
			}
		}
		if (!config.counterClockwise) {
			ea[0] = -ea[0]; ea[1] = -ea[1]; ea[2] = -ea[2];
		}
		if (config.frameRelative) { var t = ea[0]; ea[0] = ea[2]; ea[2] = t; }
		return ea;
	}
};